//
// Created by wyj on 2025/5/7.
//

// pid 算法
#include "studio_macros.h"
#include "control/pid/studio_pid.h"

// 路径压缩算法
#include "geography/studio_proj.h"
#include "geometry/studio_geo_utils.h"
#include "geometry/studio_geo_algo.h"
#include "geography/studio_proj.h"
#include "gdal_priv.h"
#include "ogrsf_frmts.h"
#include "geometry/studio_vector_raster.h"

// int main()
// {
//     printf("===================== %s =====================\n", __FILE__);
//
//     studio_geo_utils::init_gdal_env();
//     std::string path;
// #ifdef IS_WINDOWS
//     path = "D:/9_data/2_readfile/geojson/multi_point/fitting_examples.geojson";
// #else
//     path = "/home/wyj/myself/2_data/2_geojson/multi_point/fitting_examples.geojson";
// #endif
//
//     std::cout << "path: " << path << std::endl;
//     std::vector<studio_geo_coll> res_collections;
//     std::vector<studio_geo_coll> collections;
//     studio_geo_utils::read_geo_coll(path, collections);
//     for (auto& coll : collections)
//     {
//         // 转换为高斯投影
//         studio_line gauss_line;
//         proj proj_test;
//         double central = static_cast<int>(coll.m_line[0].x / 3) * 3;
//         for (auto& point : coll.m_line)
//         {
//             double gx = 0.0;
//             double gy = 0.0;
//             proj_test.lonlat_to_gauss(central, point.x, point.y, gx, gy);
//             gauss_line.push_back(studio_point(gx, gy));
//         }
//
//         // 简化线段，目标点数为28个
//         int max_points = 28;
//         double epsilon = 1.0;
//         // studio_line gs_simplified_line = simplify_line_2(gauss_line, max_points);
//         studio_line gs_simplified_line;
//         bool res = geo_vacuate::vacuate(gauss_line, max_points,epsilon, gs_simplified_line);
//         if (!res)
//         {
//             std::cout << "Failed to simplify line." << std::endl;
//             return 1;
//         }
//         studio_line simplified_line;
//         // 高斯投影在转回经纬度
//         for (auto& point : gs_simplified_line)
//         {
//             double lon = 0.0;
//             double lat = 0.0;
//             proj_test.gauss_to_lonlat(central, point.x, point.y, lon, lat);
//             simplified_line.push_back(studio_point(lon, lat));
//         }
//
//         studio_geo_coll temp;
//         temp.m_type = enum_geometry_type::egtLineString;
//         temp.m_line = simplified_line;
//         res_collections.push_back(temp);
//         break;
//     }
//     std::string output_path;
// #ifdef IS_WINDOWS
//     output_path = "D:/9_data/2_readfile/geojson/multi_point/fitting_examples_res_1.geojson";
// #else
//     output_path = "/home/wyj/myself/2_data/2_geojson/multi_point/fitting_examples_res_1.geojson";
// #endif
//     studio_geo_utils::write_geo_coll(output_path, res_collections);
//
//
//     studio_geo_utils::destroy_gdal_env();
//     return 0;
// }


// /////////// PID 算法  ///////////
// int main()
// {
//     double ini = 2, goal = 50, p = 0.5, i = 0.001, d = 0.6;
//     studio_pid pid(ini, goal, p, i, d);
//     pid.set_integral_limit(100);
//     pid.set_output_limit(200);
//
//     // 第一阶段控制到 50
//     double current_value = ini;
//     for (int i = 0; i < 20; ++i)
//     {
//         double output = pid.compute(current_value);
//         current_value += output; //* dt;
//         std::cout << "Step " << i + 1 << ": 目标=" << pid.get_goal() << ", 当前值=" << current_value << ", 输出=" << output <<
//                 std::endl;
//     }
//
//     std::cout << "到达目标值，开始第二阶段控制" << std::endl;
//     // 改变目标值并重置PID状态
//     pid.set_goal(100);
//
//     // 第二阶段控制到 100
//     for (int i = 0; i < 20; ++i)
//     {
//         double output = pid.compute(current_value);
//         current_value += output; //* dt;
//         std::cout << "Step " << i + 1 << ": 目标=" << pid.get_goal() << ", 当前值=" << current_value << ", 输出=" << output <<
//                 std::endl;
//     }
// }

/////////// 扫描线 算法  ///////////

// 创建一个网格矢量
OGRPolygon* createGridCell(double left, double bottom, double cell_size)
{
    OGRPolygon* polygon = new OGRPolygon();
    OGRLinearRing* ring = new OGRLinearRing();
    ring->addPoint(left, bottom);
    ring->addPoint(left + cell_size, bottom);
    ring->addPoint(left + cell_size, bottom + cell_size);
    ring->addPoint(left, bottom + cell_size);
    ring->addPoint(left, bottom);

    polygon->addRing(ring);
    return polygon;
}

// 生成网格文件 geojson 文件
void generateGridAndSave(const char* filename, const double src_top, const double src_bottom, const double src_left, const double src_right, const double cell_size)
{
    GDALDriver* driver = GetGDALDriverManager()->GetDriverByName("GeoJSON");
    if (driver == nullptr)
    {
        std::cerr << "GeoJSON driver not available." << std::endl;
        return;
    }
    GDALDataset* dataset = driver->Create(filename, 0, 0, 0, GDT_Unknown, nullptr);
    if (dataset == nullptr)
    {
        std::cerr << "Failed to create output file." << std::endl;
        return;
    }
    OGRSpatialReference srs;
    srs.SetWellKnownGeogCS("WGS84");
    OGRLayer* layer = dataset->CreateLayer("grid", &srs, wkbPolygon, nullptr);
    if (layer == nullptr)
    {
        std::cerr << "Failed to create layer." << std::endl;
        return;
    }
    OGRFieldDefn fieldDefn("ID", OFTInteger);
    layer->CreateField(&fieldDefn);
    int id = 0;


    double left = std::floor(src_left / cell_size) * cell_size;
    double right = std::ceil(src_right / cell_size) * cell_size;
    double top = std::ceil(src_top / cell_size) * cell_size;
    double bottom = std::floor(src_bottom / cell_size) * cell_size;


    for (double y = bottom; y < top; y += cell_size)
    {
        for (double x = left; x < right; x += cell_size)
        {
            OGRPolygon* gridCell = createGridCell(x, y, cell_size);
            OGRFeature* feature = OGRFeature::CreateFeature(layer->GetLayerDefn());
            feature->SetField("ID", id++);
            feature->SetGeometry(gridCell);
            layer->CreateFeature(feature);
            OGRFeature::DestroyFeature(feature);
            delete gridCell;
        }
    }
    GDALClose(dataset);
}

int main()
{
    std::cout << "\r\n\r\n****************" << "SCANNING_LINE" << "****************" << std::endl;
     studio_geo_utils::init_gdal_env();

    std::string fence_1_path = "/home/wyj/myself/2_data/2_geojson/fence_1.geojson";
    double central=0.0;
    std::vector<studio_geo_coll> collections;
    studio_poly gs_src_poly;
    bool re = studio_geo_utils::read_geo_coll(fence_1_path, collections);
    double left = 1000, right = -1000, top = -1000, bottom = 1000;
    double gs_left = 100000000, gs_right = -1000, gs_top = -1000, gs_bottom = 100000000;
    for (auto& coll : collections)
    {
        if (coll.m_type == enum_geometry_type::egtPolygon)
        {
            if (coll.m_poly.outer_ring.points.empty())
            {
                break;
            }
            central = floor((coll.m_poly.outer_ring.points.begin()->x + 1.5) / 3.0) * 3.0;
            for (const auto& point: coll.m_poly.outer_ring.points)
            {
                double t_lon = point.x;
                double t_lat = point.y;
                left = std::min(left, t_lon);
                right = std::max(right, t_lon);
                top = std::max(top, t_lat);
                bottom = std::min(bottom, t_lat);
                // 经纬度转高斯
                double gx = 0.0, gy = 0.0;
                proj::lonlat_to_gauss(central, point.x, point.y, gx, gy);
                gs_src_poly.outer_ring.points.push_back(studio_point(gx, gy));

                gs_left = std::min(gs_left, gx);
                gs_right = std::max(gs_right, gx);
                gs_top = std::max(gs_top, gy);
                gs_bottom = std::min(gs_bottom, gy);
            }
        }
        break;
    }

    // // 经纬度计算
    studio_rect rect(left,top,right,bottom);
    x_scan_line xlr;
    // scan_line_algo xlr;
    double cell_size = 0.0001;
    xlr.set(rect, cell_size);
    xlr.rasterize(collections.begin()->m_poly);
    xlr.fill();
    // xlr.scanning(collections.begin()->m_poly);
    // 生成中线点并存储到 studio_multi_point 对象
    studio_multi_point grid_mid_points;
    xlr.get_grid_mid_points(grid_mid_points);
    std::vector<studio_geo_coll> out_collections;
    studio_geo_coll temp_geo_coll;
    temp_geo_coll.m_type = enum_geometry_type::egtMultiPoint;
    temp_geo_coll.m_points = grid_mid_points;
    out_collections.push_back(temp_geo_coll);
    studio_geo_utils::write_geo_coll("/home/wyj/myself/2_data/2_geojson/scan/scanning_gcs_2.geojson", out_collections);

    // 高斯计算
    studio_rect gs_rect(gs_left,gs_top,gs_right,gs_bottom);
    x_scan_line xlr_gs;
    // scan_line_algo xlr_gs;
    double gs_cell_size = 10;
    xlr_gs.set(gs_rect, gs_cell_size);
    xlr_gs.rasterize(gs_src_poly);
    xlr_gs.fill();
    // xlr_gs.scanning(gs_src_poly);
    // 生成中线点并存储到 studio_multi_point 对象
    studio_multi_point gs_grid_mid_points;
    xlr_gs.get_grid_mid_points(gs_grid_mid_points);
    // 高斯转经纬度
    studio_multi_point gcs_grid_mid_points;
    for (auto& point : gs_grid_mid_points)
    {
        double lon = 0.0, lat = 0.0;
        proj::gauss_to_lonlat(central, point.x, point.y, lon, lat);
        gcs_grid_mid_points.push_back(studio_point(lon, lat));
    }
    std::vector<studio_geo_coll> gs_out_collections;
    studio_geo_coll gs_temp_geo_coll;
    gs_temp_geo_coll.m_type = enum_geometry_type::egtMultiPoint;
    gs_temp_geo_coll.m_points = gcs_grid_mid_points;
    std::vector<studio_geo_coll> temp_gs_geo_coll;
    temp_gs_geo_coll.push_back(gs_temp_geo_coll);
    studio_geo_utils::write_geo_coll("/home/wyj/myself/2_data/2_geojson/scan/scanning_gs_2.geojson", temp_gs_geo_coll);

    generateGridAndSave("/home/wyj/myself/2_data/2_geojson/scan//grid_1.geojson", top, bottom, left, right, cell_size);




    int b=0;

// {    studio_poly gs_src_poly;
     // double left = 100000000, right = -1000, top = -1000, bottom = 100000000;
     // for (auto& coll : collections)
     // {
     //     if (coll.m_type == enum_geometry_type::egtPolygon)
     //     {
     //         if (coll.m_poly.outer_ring.points.empty())
     //         {
     //             std::cout << "Polygon has no points." << std::endl;
     //             break;
     //         }
     //         double central = floor((coll.m_poly.outer_ring.points.begin()->x + 1.5) / 3.0) * 3.0;
     //         for (const auto& point: coll.m_poly.outer_ring.points)
     //         {
     //             double t_lon = point.x;
     //             double t_lat = point.y;
     //             // 经纬度转高斯
     //             double gx = 0.0;
     //             double gy = 0.0;
     //             proj::lonlat_to_gauss(central, point.x, point.y, gx, gy);
     //             gs_src_poly.outer_ring.points.push_back(studio_point(gx, gy));
     //
     //             left = std::min(left, gx);
     //             right = std::max(right, gx);
     //             top = std::max(top, gy);
     //             bottom = std::min(bottom, gy);
     //         }
     //     }
     //     break;
     // }
//
//     studio_rect rect(left,top,right,bottom);
//     scan_line_algo xlr;
//     double cell_size = 10;
//     xlr.set(rect, cell_size);
//     xlr.scanning(gs_src_poly);
//     // xlr.fill();
// }

    // generateGridAndSave("/home/wyj/myself/2_data/2_geojson/grid_1.geojson", top, bottom, left, right, cell_size);


    geo_utils::destroy_gdal_env();

    return 0;

//     std::filesystem::path geo_path("/home/wyj/myself/2_data/2_geojson/scan");
//
//
//     std::filesystem::path grid = geo_path;
//     std::filesystem::path m_lines = geo_path;
//     std::filesystem::path point_one = geo_path;
//     std::filesystem::path point_more = geo_path;
//     grid += "/grid.geojson";
//     m_lines+="/src_mmlines.geojson";
//     point_one += "/src_point.geojson";
//     point_more += "/src_points.geojson";
//
//     studio_geo_coll multiPolygonData;
//
//
//     studio_multi_poly multi_poly;
//     std::vector<studio_geo_coll> multiPolygonDataVec;
//
//     // ============ 单点 ==================
//     studio_point point0 = studio_point(113.32007, 29.51516);
//     multiPolygonData.m_type = enum_geometry_type::egtPoint;
//     multiPolygonData.m_point = point0;
//     multiPolygonDataVec.push_back(multiPolygonData);
//     // studio_geo_utils::write_geo_coll(point_one.string(), multiPolygonDataVec);
//     scan_line_algo xlr_op;
//     studio_rect rect(113.25921, 29.54244, 113.44758, 29.40021);
//     xlr_op.set(rect, 0.025);
//     xlr_op.scanning(point0);
//     // xlr_op.fill();
//
//
//     // ============ 多点 ==================
//     studio_point point1 = studio_point(113.34245, 29.51446);
//     studio_point point2 = studio_point(113.38534, 29.49290);
//     studio_point point3 = studio_point(113.38909, 29.46805);
//     studio_point point4 = studio_point(113.36683, 29.45751);
//     studio_point point5 = studio_point(113.33683, 29.46102);
//     studio_point point6 = studio_point(113.28784, 29.49055);
//     studio_point point7 = studio_point(113.28784, 29.51540);
//     studio_point point8 = studio_point(113.41417, 29.46360);
//     studio_point point9 = studio_point(113.38745, 29.51329);
//     studio_point point10 = studio_point(113.41417, 29.51212);
//     multiPolygonData.m_type = enum_geometry_type::egtMultiPoint;
//
//     multiPolygonData.m_points = (std::vector<studio_point>{ point1, point2, point3, point4, point5,point6, point7,point8,point9, point10 });
//     multiPolygonDataVec.push_back(multiPolygonData);
//     // studio_geo_utils::write_geo_coll(point_more.string(), multiPolygonDataVec);
//     scan_line_algo xlr_mp;
//     xlr_mp.set(rect, 0.025);
//     xlr_mp.scanning(multiPolygonData.m_points);
//     // xlr_mp.fill();
//
//     generateGridAndSave(grid.string().c_str(), 29.54244, 29.40021, 113.25921, 113.44758, 0.025);
//     int a=0;
//
//
//     // ============ 多线 ==================
//
//     // 创建第一条线
//     studio_line line1 (std::vector<studio_point> {studio_point(113.32007, 29.51516),
//         studio_point(113.34585, 29.51329),
//         studio_point(113.36905, 29.49079),
//         studio_point(113.35148, 29.48305),
//         studio_point(113.39554, 29.46899)});
//
//     // 创建第二条线
//
//     studio_line line2 = (std::vector<studio_point>{
//         studio_point(113.29593, 29.44180),
//         studio_point(113.34140, 29.44063),
//         studio_point(113.37491, 29.46735),
//         studio_point(113.40093, 29.48727),
//         studio_point(113.41851, 29.44649)
//     });
//
//     // 创建多线条集合
//     studio_multi_line multi_line;
//     multi_line.push_back(line1);
//     multi_line.push_back(line2);
//     multiPolygonData.m_type = enum_geometry_type::egtMultiLineString;
//     multiPolygonData.m_lines = multi_line;
//
//     //multiPolygonDataVec.push_back(multiPolygonData);
//     //studio_geo_utils::write_geo_coll(m_lines.string(), multiPolygonDataVec);
//
//
//     x_scan_line xlr_mml;
//     // xlr_mml.top = 29.54244;
//     // xlr_mml.bottom = 29.40021;
//     // xlr_mml.left = 113.25921;
//     // xlr_mml.right = 113.44758;
//     // xlr_mml.cell_size = 0.00001;
//     // xlr_mml.rasterization(multi_line);
//
//
//     int mml = 0;
//
//
//     // 第一个面
//     studio_poly first_poly;
//     first_poly.outer_ring.points =
//     {
//         studio_point(113.30624, 29.48780),
//         studio_point(113.36182, 29.51534),
//         studio_point(113.41794, 29.49356),
//         studio_point(113.41668, 29.41501),
//         studio_point(113.30606, 29.41483),
//         studio_point(113.30624, 29.48780)
//     };
//     first_poly.outer_ring.is_outer = 1;
//     // multi_poly.push_back(first_poly);
//
//     // 第二个面
//     studio_poly second_poly;
//     second_poly.outer_ring.points =
//     {
//         studio_point(113.28985, 29.44375),
//         studio_point(113.41716, 29.43845),
//         studio_point(113.43422, 29.41111),
//         studio_point(113.29779, 29.40876),
//         studio_point(113.28985, 29.44375)
//     };
//     second_poly.outer_ring.is_outer = 1;
//     // multi_poly.push_back(second_poly);
//     //  添加到多面数据中
//     //multiPolygonData.m_polys.push_back(first_poly);
//      //multiPolygonData.m_polys.push_back(second_poly);
//
//     multiPolygonData.m_type = enum_geometry_type::egtMultiPolygon;
//
//     //multiPolygonDataVec.push_back(multiPolygonData);
//
//     // studio_geo_utils::write_geo_coll("./shp/src_mmpolys.geojson", multiPolygonDataVec);
//
//     // x_scan_line xlr;
//     // xlr.top = 29.54244;
//     // xlr.bottom = 29.40021;
//     // xlr.left = 113.25921;
//     // xlr.right = 113.44758;
//     // xlr.cell_size = 0.025;
//     // xlr.rasterization(first_poly);
//
//
//
//     // generateGridAndSave("./shp/grid.geojson", xlr.top, xlr.bottom, xlr.left, xlr.right, xlr.cell_size);
//
//
//     geo_utils::destroy_gdal_env();

    int e = 0;
    int f = 9;
    int g = 8;
}



